typedef struct motor
{
        int speed;
        int duty;
        int speed_target;
        int16 speed_target_default;
        float P;
        float I;
        float D;
        float I_COUNT_MAX;
        uint8 sign;
};
typedef struct servo
{
        int duty;
        float P;
        float I;
        float D;
        float I_COUNT_MAX;
        float angle;
        float last_angle;
};
typedef struct momentum_wheel
{
        float Balance_P;
        float Balance_D;
        float Center_of_gravity;
        float Center_of_gravity_default;
};
typedef struct series
{
        float gyro_P;
        float gyro_D;
        float angle_P;
        float angle_D;
        float speed_P;
        float speed_I;
        float speed_D;
};
typedef struct tkf
{
        float Q_angle;
        float Q_gyro;
        float R_angle;
        float dt;       //dtΪkalman˲ʱ
        char  C_0;
        float Q_bias, Angle_err;
        float PCt_0, PCt_1, E;
        float K_0, K_1, t_0, t_1;
        float Pdot[4];
        float PP[2][2];
};

#define wheelbase 0.175     //

#define straight_flag !(cross_flag /*+ corner_flag*/ + roundabout_flag + garage_flag_lap1 + garage_flag_lap2 + fork_flag + ramp_flag)

typedef struct line_info
{
    int left_edge;
    int right_edge;
    int center;
    int width;

    int left_found_flag;
    int right_found_flag;

    int left_dif_1st;
    int left_dif_2nd;
    int right_dif_1st;
    int right_dif_2nd;
};
typedef struct frame_info
{
    int both_lost_lines;
    int left_lost_lines;
    int right_lost_lines;
    int break_line;
    int last_break_line;
    int left_inf_pts;
    int right_inf_pts;
    int inf_pts;
    int zebra;
    int left_max_2nd;
    int right_max_2nd;
};
#define DIFTH 10
#define DIFTH_LOW 7

#define SW1 P11_10
#define SW1_ON gpio_get(SW1) == 0
#define SW2 P11_3
#define SW2_ON gpio_get(SW2) == 0
#define SW3 P11_6
#define SW3_ON gpio_get(SW3) == 0
#define SW4 P11_2
#define SW4_ON gpio_get(SW4) == 0

#define IMG_PIXEL(x,y) (image_dec[y][x] == 0)
//#define IMG_PIXEL(x,y) !((*(image_bin[0]+y*20+x/8)>>(7-x%8))&1)
//#define set_black(x,y) *(image_bin[0]+y*20+x/8)&=~(1<<(7-x%8))
#define set_black(x,y) (image_dec[y][x] = 0)

#define bldc_speed_now speed_filter.data_average

#define RIGHT 1
#define LEFT 0

typedef struct speed
{
        float P;
        float I;
        float D;
        float I_COUNT_MAX;
        float speed_add;
};
typedef struct angle_expect
{
        float P;
        float I;
        float D;
        float I_COUNT_MAX;
        float angle;
};

#define MAX_DUTY 2500


// sign function
#define _sign(a) ( ( (a) < 0 )  ?  -1   : ( (a) > 0 ) )
#define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f))
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define _isset(a) ( (a) != (NOT_SET) )
#define _UNUSED(v) (void) (v)

// utility defines
#define _2_SQRT3 1.15470053838f
#define _SQRT3 1.73205080757f
#define _1_SQRT3 0.57735026919f
#define _SQRT3_2 0.86602540378f
#define _SQRT2 1.41421356237f
#define _120_D2R 2.09439510239f
#define _PI 3.14159265359f
#define _PI_2 1.57079632679f
#define _PI_3 1.0471975512f
#define _2PI 6.28318530718f
#define _3PI_2 4.71238898038f
#define _PI_6 0.52359877559f
#define _RPM_TO_RADS 0.10471975512f

#define BEEP buzz = 0
#define BEEP_temp buzz = 0
#define BEEP_temp_2 buzz = 0



#define ACC accelerate(0)
#define ACC_clr accelerate(CLEAR)
#define CLEAR 1
